# Ivo's implemetation of Ellipse class
class Ellipse:
C : np.array #homogeneous conic matrix x'*C*x=0
def __init__( self, C : np.array ):
self.C = C
@classmethod
def estimate_ellipsoid( cls, points : np.array ):
"""
C = estimate_ellipsoid_2D ( points )
----------------------------------
estimates a homogeneous ellipsoid C
x'*C*x = 0
from a number of point samples
(in a least squares sense)
format of points
[ x^1_1 . . . . . x^N_1 ]
[ . . ]
points = [ . . ]
[ . . ]
[ x^1_n . . . . . x^N_n ]
[ 1 . . . . . 1 ]
where each point is element R^n
and there are N points, the points
have to be in homogeneous coordinates.
"""
n = points.shape[0]
num_entries = int((n+1) * n/2)
if points.shape[1] < num_entries-1:
print('insufficient number of points, need at least {}'.format( num_entries-1 ) )
# -------------- estimate ellipse ---------------
# assemble linear system
A = np.zeros( [points.shape[ 1 ], num_entries ] )
#for all points
for i in range( 0, points.shape[ 1 ] ):
#extract point
x = np.array([points[:, i]]).T
#print(x)
X = np.matmul( x, x.T )
#print(X)
X2 = X.T + X - np.diag( np.diag( X ) )
#print(X2)
row = X2[ np.where( np.triu( np.ones( X2.shape ) ) ) ]
#print(row)
A[ i, : ] = row
#print(A)
#solve for null space
U,D,V = np.linalg.svd( A )
ell = V[-1,:]
#create conic matrix
Cd = np.zeros( X2.shape )
Cd[ np.where( np.triu( np.ones( X2.shape ) ) ) ] = ell
C = Cd + Cd.T - np.diag( np.diag ( Cd ) )
#detC = np.linalg.det( C[0:-1,0:-1] )
#print(detC)
if Ellipse.is_hyperboloid( C ):
print('Warning: result is a hyperboloid!')
if Ellipse.is_paraboloid( C ):
print('Warning: result is a paraboloid!')
return C
@classmethod
def cov_mean_to_ellipsoid( cls, cov, mean, k ):
"""
C = cov_mean_to_ellipsoid_homog ( Cov, Mean, k )
----------------------------------
generates a homogeneous ellipsoid C
x'*C*x = 0
from covariance matrix and mean value
of a gaussian distribution
k is a confidence factor determibed by the chi^2
distribution. The probability that x is inside
the ellipsoid is P_{chi^2}(k,2). Some values
for k are 2.41 (70%) and k=5.99 (95%).
cov : np.array - covariance matrix (non-homogeneous)
mean : np.array - mean value (vector)
k : float - confidence factor, see above
"""
#column vector, assume we get a 1D array
mean = np.array( [mean] ).T
SIGMAinv = np.linalg.inv( cov )
lastentry = np.matmul( mean.T, np.matmul( SIGMAinv, mean ) ) - k
C = np.vstack( [ np.hstack( [ SIGMAinv, -np.matmul(SIGMAinv, mean)] ),
np.hstack( [ -np.matmul(mean.T,SIGMAinv), lastentry] ) ] )
return C
@classmethod
def srt_to_ellipsoid( cls, S, R, T ):
"""
C = srt_to_ellipsoid_homog ( S, R, T )
----------------------------------
generates a homogeneous ellipsoid C
x'*C*x = 0
from translation, rotation and scale parts as
x'* (T^-1 * R^-1 * S^-1) ( S * R * T ) x = 0
see: Ivo Ihrke: "Some Notes on Ellipses", TechReport, 2004
"""
#function C = srt_to_ellipsoid_homog ( S, R, T )
invT = np.linalg.inv( T )
invR = R.T
invS = np.linalg.inv( S )
left = np.matmul( invT.T, np.matmul( invR.T, invS.T ) )
right = np.matmul( invS, np.matmul( invR, invT ) )
C = np.matmul( left, right )
return C
@classmethod
def decompose_ellipsoid( cls, C ):
"""
[S,R,T] = decompose_ellipsoid_homog ( C )
----------------------------------
decomposes a homogeneous ellipsoid C
x'*C*x = 0
into translation, rotation and scale parts
x'* (T^-1 * R^-1 * S^-1) ( S * R * T ) x = 0
Note that C \neq (T^-1 * R^-1 * S^-1) ( S * R * T )
in some cases because the orientation of
the ellipsoid's major axis is not fixed.
The function generates a right handed system.
"""
#print(C)
C_ih = C[0:-1,0:-1]
#print(C_ih)
t = -np.matmul( np.linalg.pinv( C_ih ), C[0:-1,-1] )
T = np.eye( C.shape[ 0 ] )
T[0:-1, -1 ] = t
U,D,V = np.linalg.svd( C_ih )
R = np.eye( C.shape[ 0 ] )
R[0:-1,0:-1] = V
#S = R' * T' * C * T * R;
S = np.matmul( R.T, np.matmul( T.T, np.matmul( C, np.matmul( T, R ) ) ) )
S = S / S[-1,-1]
S = np.sqrt( np.diag(1/abs(np.diag(S))))
return S, R, T
@classmethod
def is_ellipsoid( cls, C ):
"""
bool = is_ellipsoid_homog ( C )
----------------------------------
determines whether homogeneous conic C
x'*C*x = 0
is an ellipsoid
"""
#function val = conic_is_ellipsoid_homog ( C )
detC = np.linalg.det( C[0:-1,0:-1] )
return detC > 0
@classmethod
def is_hyperboloid( cls, C ):
"""
bool = is_hyperboloid_homog ( C )
----------------------------------
determines whether homogeneous conic C
x'*C*x = 0
is a hyperboloid
"""
detC = np.linalg.det( C[0:-1,0:-1] )
return detC < 0
@classmethod
def is_paraboloid( cls, C ):
"""
bool = is_paraboloid_homog ( C )
----------------------------------
determines whether homogeneous conic C
x'*C*x = 0
is a paraboloid
"""
detC = np.linalg.det( C[0:-1,0:-1] )
#maybe with EPS ?
return detC == 0
def get_center( self ):
S,R,T = self.decompose_ellipsoid( self.C )
return [ T[2,0], T[2,1] ]
def plot( self, ax, **kwargs ):
"""
plot_ellipsoid_2D ( C, style, N )
----------------------------------
plots a homogeneous ellipsoid C
x'*C*x = 0
style is a matlab plot style, e.g. 'b-' (default)
N is the number of polygon segments to
approximate the ellipse
"""
#function plot_ellipsoid_homog_2d ( C, style, N )
if not 'style' in locals():
style = 'b-'
if not 'N' in locals():
N = 100
angles = np.linspace( -np.pi, np.pi, N )
# generate a unit circle or a unit hyperbola
if ( Ellipse.is_ellipsoid( self.C ) ):
#uc = [ cos(x); sin(x); ones(size(x)) ];
x = np.cos( angles )
y = np.sin( angles )
w = np.ones( angles.shape )
uc = np.vstack([x,y,w])
else:
EPS = 0.1
x = np.linspace( -np.pi/2+EPS, np.pi/2-EPS, int( N/2 ) )#-(pi/2-EPS):((pi-2*EPS)/(N/2)):(pi/2-EPS);
x2 = np.linspace( np.pi/2+EPS, 3*np.pi/2-EPS, int( N/2 ) ) #x2 = (pi/2+EPS):((pi-2*EPS)/(N/2)):(3*pi/2-EPS);
uc = np.vstack( [ 1/np.cos(x), np.tan(x), np.ones( x.shape ) ] )
uc2 = np.vstack( [ 1/np.cos(x2), np.tan(x2), np.ones( x2.shape ) ] )
#decompose
S,R,T = Ellipse.decompose_ellipsoid( self.C )
#transform unit circle or parabola
transform = np.matmul( T, np.matmul( R, S ) )
ell = np.matmul( transform, uc )
if 'uc2' in locals():
ell2 = np.matmul( transform, uc2 )
ax.plot( ell2[0,:], ell2[1,:], style )
#plot conic
ax.plot( ell[0,:], ell[1,:], style )
#plot coordinate system
tx = T[0,2]
ty = T[1,2]
# print(tx, ty)
smin = S[0,0]
smaj = S[1,1]
ax.plot( np.array( [ tx, tx+smin*R[0,0] ] ), np.array( [ ty, ty+smin*R[1,0] ] ), 'r' )
ax.plot( np.array( [ tx, tx+smaj*R[0,1] ] ), np.array( [ ty, ty+smaj*R[1,1] ] ), 'b' )
ax.set_aspect('equal', adjustable='box')
return (tx, ty)
